/*
 * @Author: ling
 * @Date: 2025-05-28 16:29:34
 * @LastEditors: lingeros
 * @LastEditTime: 2025-05-30 16:19:21
 * @Description:
 * Copyright © Dongguan Xiaotun Intelligent Technology Co., Ltd. All Rights Reserved
 * Address: Room 1201, Building G4, Songshan Lake International Entrepreneurship and Innovation Community, Dongguan City
 */
#ifndef XTBOATMODEL1_H
#define XTBOATMODEL1_H
#include "XTBoat.h"
#include "XTBoatPID.h"
#include <cstring>
#include <termios.h>

namespace xtai_simulation
{

    class XTBoatModel1 : public XTBoat
    {
    public:
        
        XTBoatModel1(float prephi);
        XTBoatModel1(double baseLat, double baseLon, float prephi);
        XTBoatModel1() = default;
        ~XTBoatModel1() = default;
        void SetEnviro(double mp, double v_c, double beta_c);
        void DynamicCalculate(const std::vector<float> input);
        std::vector<double> StateGet();
        double n[2]; // propeller revolutions (rps), n = [ n_left n_right ]'
    private:
        XTBoatPID headingcontroller;
        XTBoatPID velocitycontroller;
        double x[12]; // initial values for x = [ u v w p q r x y z phi theta psi ]'
        double xdot[12];
        double outx[12];
        double h; // sampling time [s]
        double U;
        double mp;     // payload mass (kg), max value 45 kg
        double rp[3];  // location of payload (m)
        double v_c;    // current speed (m/s)
        double beta_c; // current direction (rad)
        void Dynamic(const double x[12], const double n[2], double mp,
                     const double rp[3], double V_c, double beta_c,
                     double xdot[12], double *U);
        double lat2m = 0;
        double lng2m = 0;
    };

} 

#endif 